#include "ROB_Telemetry.h"
#include "ROB_i2cBusMaster.h"
#include "ROB_Motor.h"




void CMotor::Begin(void) {
  _mCtrlCmd[0] = MOT_CMD_SPEED | MOT_A_STOP | MOT_A_STOP;
  _mCtrlCmd[1] = 0;
  _mCtrlCmd[2] = 0;
}



uint8_t CMotor::update(void) {

  // check if we received a sensor update

  if(I2cBusmaster.ackReceived(I2CMCTRL_ADR, I2CMCTRL_ACK_LEN)) {
    uint8_t * rcvBuf = I2cBusmaster.getAck();
    tmy.batteryStatus = rcvBuf[0];
    tmy.engineLSpeed = rcvBuf[1];
    tmy.engineRSpeed = rcvBuf[2];
  }


  // send actor update and request sensor update

  if(_lastEngineDirections != tmy.engineDirections ||
    _lastEngineLPower != tmy.engineLPower || 
    _lastEngineRPower != tmy.engineRPower) {


    _mCtrlCmd[0] = MOT_CMD_SPEED | tmy.engineDirections;
    _mCtrlCmd[1] = tmy.engineLPower;
    _mCtrlCmd[2] = tmy.engineRPower;
    // when sending to motor controller 3 bytes, 
    // it will return 3 bytes: batterystatus, engineLSpeed, engineRSpeed
    // this is handled by I2cBusCtrl
    if(I2cBusmaster.send(I2CMCTRL_ADR, 3, I2CMCTRL_ACK_LEN, _mCtrlCmd) == I2C_ERROR_NONE) {

      _lastEngineDirections = tmy.engineDirections ;
      _lastEngineLPower = tmy.engineLPower;
      _lastEngineRPower = tmy.engineRPower;

    } 
    else {
      return I2C_ERROR_LOCKED;
    }

  } 

  return I2C_ERROR_NONE;
}




uint8_t CMotor::requestSensors(void) {
  _mCtrlCmd[0] = MOT_CMD_SENSE;
  if(I2cBusmaster.send(I2CMCTRL_ADR, 1, I2CMCTRL_ACK_LEN, _mCtrlCmd) != I2C_ERROR_NONE) 
    return I2C_ERROR_LOCKED;
  return I2C_ERROR_NONE;
}




CMotor Motor;

